package ch.areion.areionapp.core;

import android.app.Activity;
import ch.areion.areionapp.IntentIntegrator;
import ch.areion.areionapp.Main;
import ch.areion.areionapp.communication.Monitor;
import ch.areion.areionapp.engine.Engine;
import ch.areion.areionapp.engine.IEngine;
import ch.areion.areionapp.io.BoxNumber;
import ch.areion.areionapp.io.IIO;
import ch.areion.areionapp.io.IO;
import ch.areion.areionapp.periphery.BatteryControl;
import ch.areion.areionapp.sensor.IIRSensor.IRS;
import ch.areion.areionapp.sensor.IUSSensor;
import ch.areion.areionapp.sensor.USSensor;
import ch.areion.common.message.Module;
import ch.areion.common.message.Type;
import ch.areion.common.message.Version;
import ch.areion.areionapp.sensor.*;

import ioio.lib.api.DigitalInput;
import ioio.lib.api.DigitalOutput;
import ioio.lib.api.exception.ConnectionLostException;
import ioio.lib.util.IOIOLooper;
import ioio.lib.api.IOIO;

/**
 * 
 * @author IljasaRedzepi
 * 
 */
public class Controller extends Activity implements IOIOLooper {
	private BatteryControl battery;
	private static IEngine engine;
	private IUSSensor uss;
	private IIRSensor irs;
	private static IIO io;

	private IOIO ioio;
	private DigitalOutput led; // The on-board LED
	private static IOIOLooper looper = null;

	private static String boxnumber = "2";

	private final static boolean USESLEEP = false;

	public final static int SPEED_SLOWEST = 4;
	public final static int SPEED_SLOW = 100;
	public final static int SPEED_NORMAL = 250;
	public final static int SPEED_FAST = 450;
	public final static int TICKS_CURVE_90_DEGREES = 3300;
	public final static int TICKS_CURVE_45_DEGREES = 1650;
	public final static int TICKS_BOX_DRIVE_IN = 1900;
	public final static int GAP_START = 400;
	public final static int GAP_BOX_1 = 285;
	public final static int GAP_BOX_2 = 570;
	public final static int GAP_BOX_3 = 900;
	public final static int GAP_BOX_4 = 1200;
	public final static int GAP_BOX_5 = 1525;
	public final static double LINE_TOLERANCE = 100;
	public final static double LINE_TOLERANCE_EXACT = 20;
	public final static double LINE_CORRECTION_FACTOR = 9;
	public final static double LINE_CORRECTION_FACTOR_FINE_TUNING = 1.2;
	public final static double ANGLE_TOLERANCE = 5;
	public final static double ANGLE_TOLERANCE_EXACT = 3;

	/**
	 * 
	 */
	public Controller() {
		Controller.looper = this;
	}

	public static IOIOLooper getLooper() {
		return Controller.looper;
	}

	/**
	 * @throws ConnectionLostException
	 *             When IOIO connection is lost.
	 * @throws InterrruptedException
	 *             When thread is interrupted.
	 */
	public void start() throws ConnectionLostException, InterruptedException {

		//Thread.sleep(3000); // Wait after start signal.

		// Zur QR-Wand fahren
		if (USESLEEP)
			Thread.sleep(500);
		driveToQrWall();

		// Linkskurve fahren
		if (USESLEEP)
			Thread.sleep(500);
		correctQrPositionBefore();

		// an der qrcodewand fahren
		if (USESLEEP)
			Thread.sleep(500);
		driveToQrCode();

		// Starte QR-CodeScanner
		if (USESLEEP)
			Thread.sleep(500);
		readQrCode();

		// check startPrototype for the next steps.
	}

	public void startAfterQRCODE() throws ConnectionLostException,
			InterruptedException {

		// Nach ergebnis losfahren
		if (USESLEEP)
			Thread.sleep(500);
		driveToEndOfWall();

		// Linkskurve nach qrcodewand
		if (USESLEEP)
			Thread.sleep(500);
		correctQrPositionAfter();

		// int box = Integer.parseInt(boxnumber);
		if (USESLEEP)
			Thread.sleep(500);
		park();

	}

	/**
	 * @throws ConnectionLostException
	 * @throws InterruptedException
	 */
	private void driveToQrWall() throws ConnectionLostException,
			InterruptedException {
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"Process state", "1");
		engine.driveAlongWallTimed(SPEED_SLOW, 1700); // drive along
														// the wall
		engine.driveAlongWall(SPEED_NORMAL); // and drive along
		irs.waitUntil(IRS.SIDE, true); // until qr wall on the left.
		engine.driveStraightTicked(SPEED_NORMAL, 3500); // and drive another
														// ticks.
	}

	/**
	 * @throws ConnectionLostException
	 * @throws InterruptedException
	 */
	private void correctQrPositionBefore() throws ConnectionLostException,
			InterruptedException {
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"Process state", "2");
		engine.driveLeftTicked(SPEED_NORMAL, TICKS_CURVE_90_DEGREES);
		engine.correctAngle(); // check orientation
	}

	/**
	 * @throws ConnectionLostException
	 * @throws InterruptedException
	 */
	private void driveToQrCode() throws ConnectionLostException,
			InterruptedException {
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"Process state", "3.1");
		engine.driveStraight(150); // Drive slowly forward
		irs.waitUntil(IRS.SIDE, true); // ' till wall on the left.
		engine.driveStraightTicked(SPEED_NORMAL, 4000); // drive another 4k
														// ticks
	}

	/**
	 * 
	 */
	private void readQrCode() {
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"starting scanning");
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"Process state", "3.2");
		IntentIntegrator integratorScanQR = new IntentIntegrator(Main.getMain());
		integratorScanQR.addExtra("SCAN_WIDTH", 700);
		integratorScanQR.addExtra("SCAN_HEIGHT", 400);
		integratorScanQR.initiateScan();
	}

	/**
	 * @throws ConnectionLostException
	 * @throws InterruptedException
	 */
	private void driveToEndOfWall() throws ConnectionLostException,
			InterruptedException {
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"Process state", "3.3");
		engine.driveStraight(SPEED_NORMAL); // drive slowly medium speed
		irs.waitUntil(IRS.SIDE, false); // until the end of wall
		engine.driveStraightTicked(SPEED_NORMAL, 1300); // and a little bit more
	}

	/**
	 * @throws ConnectionLostException
	 * @throws InterruptedException
	 */
	private void correctQrPositionAfter() throws ConnectionLostException,
			InterruptedException {
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"Process state", "4");

		Thread.sleep(1000);

		engine.driveLeftTicked(SPEED_NORMAL, TICKS_CURVE_90_DEGREES); // drive a
																		// left
																		// curve
		engine.correctAngle(); // check orientation
		if (USESLEEP)
			Thread.sleep(2000);
		engine.driveStraight(SPEED_NORMAL); // drive medium forward
		irs.waitUntil(IRS.SIDE, true); // until wall on the left
		engine.driveStraightTicked(SPEED_NORMAL, 800); // drive a little bit
														// more
		// forward
	}

	/**
	 * @throws NumberFormatException
	 * @throws ConnectionLostException
	 * @throws InterruptedException
	 */
	private void park() throws NumberFormatException, ConnectionLostException,
			InterruptedException {
		Monitor.getInstance().add(Version.V01, Type.ERROR, Module.CORE,
				"driving box " + boxnumber);
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"Process state", "5");
//		int nr = 2;
		switch (Integer.parseInt(boxnumber)) { // switch (Integer.parseInt(boxnumber)) {
		case 1:
			engine.correctPosition(GAP_BOX_1, 2);
			break;
		case 2:
			engine.correctPosition(GAP_BOX_2, 2);
			break;
		case 3:
			engine.correctPosition(GAP_BOX_3, 2);
			break;
		case 4:
			engine.correctPosition(GAP_BOX_4, 2);
			break;
		case 5:
			engine.driveStraightTicked(SPEED_NORMAL, 1000);
			engine.correctAngle();
			engine.driveLeftTicked(SPEED_SLOW, TICKS_CURVE_90_DEGREES);
			engine.driveStraight(SPEED_SLOW);
			irs.waitUntil(IRS.SIDE, true);
			engine.driveStraightTicked(SPEED_NORMAL, 4000);                        //needs testing!!!!!!!!!!!!!!!!!
			engine.driveRightTicked(SPEED_SLOW, TICKS_CURVE_90_DEGREES);
			engine.correctAngle();
			break;
		}
		driveInBox();
	}

	/**
	 * Drive in to the box on the front.
	 * 
	 * @throws ConnectionLostException
	 * @throws InterruptedException
	 */
	private void driveInBox() throws ConnectionLostException,
			InterruptedException {

		engine.driveStraight(SPEED_NORMAL);
		boolean ok = false;
		boolean corrected = false;
		boolean irsS;
		boolean irsFL;
		boolean irsFR;
		do {
			irsS = irs.read(IRS.SIDE);
			irsFL = irs.read(IRS.FRONTLEFT);
			irsFR = irs.read(IRS.FRONTRIGHT);

			if (irsS) {
				ok = true;
			}
			if (!corrected) {
				if (irsFR) {
					if (irsFL) {
						engine.driveStraight(SPEED_NORMAL);
					} else {
						engine.rotateRight(SPEED_SLOWEST, 600);
						engine.setSpeedsAndTicks(SPEED_SLOW, SPEED_SLOWEST, 2500);
						engine.driveStraight(SPEED_NORMAL);
						corrected = true;
					}
				} else if (irsFL) {
					engine.rotateLeft(SPEED_SLOWEST, 600);
					engine.setSpeedsAndTicks(SPEED_SLOWEST, SPEED_SLOW, 2500);
					engine.driveStraight(SPEED_NORMAL);
					corrected = true;
				}
			}
		} while (!ok);

		engine.driveStraightTicked(SPEED_NORMAL, TICKS_BOX_DRIVE_IN);
	}

	public void test() throws ConnectionLostException, InterruptedException {
		engine.correctPosition(GAP_BOX_5);
	}

	/**
	 * Called every time a connection with IOIO has been established. Typically
	 * used to open pins.
	 * 
	 * @throws ConnectionLostException
	 *             When IOIO connection is lost.
	 * @throws InterrruptedException
	 *             When thread is interrupted.
	 */
	@Override
	public final void setup(IOIO ioio_) throws ConnectionLostException,
			InterruptedException {
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"A new connection with IOIO has been etablished.");
		ioio = ioio_;

		uss = new USSensor(ioio);
		irs = new IRSensor(ioio);
		engine = new Engine(ioio, uss);
		io = new IO(ioio);
		(battery = new BatteryControl(ioio, io)).start();

		led = ioio.openDigitalOutput(0, true); // initialize on-board LED
		led.write(false); // setup succeeded
		io.setDisplayBoxnumber(BoxNumber.NONE);
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.CORE,
				"IOIO successfully initialized.");
	}

	/**
	 * Called repetitively while the IOIO is connected.
	 * 
	 * @throws ConnectionLostException
	 *             When IOIO connection is lost.
	 */
	@Override
	public void loop() throws ConnectionLostException, InterruptedException {

		if(io.readCAP()){
			start();
		}
		// disconnect();

		try {
			Thread.sleep(100);
		} catch (InterruptedException e) {
		}

		// Monitor.getInstance().add(Version.V01, Type.INFO, Module.SENSOR,
		// "Starting measurements");
//		try {
//			uss.getDistanceUSSF();
//			uss.getDistanceUSSB();
//
//			irs.read(IRS.FRONTLEFT);
//			irs.read(IRS.FRONTRIGHT);
//			irs.read(IRS.SIDE);
//
//			Thread.sleep(100);
//		} catch (Exception ex) {
//			Monitor.getInstance().add(Version.V01, Type.ERROR, Module.SENSOR,
//					"" + ex);
//		}
	}

	/**
	 * Has to be called after the IOIOLooper finishes to close all open Ports of
	 * this IOIO instance.
	 */
	public void disconnect() {
		try {
			engine.disconnect();
			uss.disconnect();
			irs.disconnect();
			io.disconnect();
			led.close();
			battery.disconnect();
		} catch (Exception e) {
			Monitor.getInstance().add(Version.V01, Type.WARNING, Module.CORE,
					"Could not disconnect IOIO.");
		}
	}

	/**
	 * Called if IOIO is disconnected.
	 */
	@Override
	public void disconnected() {
		Monitor.getInstance().add(Version.V01, Type.WARNING, Module.CORE,
				"Lost connection to IOIO.");
	}

	/**
	 * Called if IOIO is not compatible with the used Android device.
	 */
	@Override
	public void incompatible() {
		Monitor.getInstance().add(Version.V01, Type.ERROR, Module.CORE,
				"IOIO is not compatible with this device.");
	}

	public static IEngine getEngine() {
		return Controller.engine;
	}

	public Controller getControllerInstance() {
		return this;
	}

	public static IIO getIO() {
		return Controller.io;
	}

	public static void setBoxNumber(String nr) {
		boxnumber = nr;
	}
}
